#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <math.h>

#include "fish.h"

/* Flocking Parameters */
const double C0 = 0.00000001; // weight of vector away from neighbors within crowding radius
const double C1 = 0.0; // weight of vector towards center of the world
const double C2 = 1.0; // weight of velocity correcting vectors towards avg. flocking velocity
const double C3 = 0.0; // weight of vector towards center of gravity
const double C4 = 0.05; // weight of random vector
const double C5 = 0.1; // weight of vector pointing to cog of local flockmates
const double R_CROWD2 = 0.00390625; // square of crowding radius
const double R_SIGHT2 = 0.015625; // square of sighting radius
const double R_SIGHT = 0.125; // sighting radius
const double BIRD_MASS = 1.0; // bird mass
const double R = 1.0; // repulsion constant
const double M = 1.0; // constant that converts vectors to acceleration
const double START_CONST = 0.5;
const double SIGHT_ANGLE = 200.0 * M_PI / 180.0;
const double SCALE_FACTOR = 1.0;
double v0x, v0y, v1x, v1y, v2x, v2y, v3x, v3y, v4x, v4y, v5x, v5y, vx, vy, ax, ay; // vectors
double cogx, cogy; // center of gravity coordinates
double local_cogx, local_cogy;


FILE *output_fp; /* Pointer to output file */
int outputp;

int n_fish = 10; /* Other defaults in fish-lib.c. */
fish_t *fish; /* Global array of fish */

struct Timer total_timer;

void init_fish();
void interact_fish(fish_t* fish_, const int n_fish, const double dt);
void bounce_fish (fish_t* fish);
void move_fish (fish_t* fish, const int n_fish, const double dt);
double compute_norm (const fish_t* fish, const int n_fish);
void output_fish(FILE* output_fp, const double t, const double dt,
		 const fish_t* fish, const int n_fish);

// made this global for interact_fish()


int
main(int argc, char **argv)
{
  double curr_time;
  double output_time;
  double dt = 0.0;
  double max_norm;
  int steps;

  get_options(argc, argv);
  srand48(clock());

  if (output_filename) {
    outputp = 1;
    output_fp = fopen(output_filename, "w");
    if (output_fp == NULL) {
      printf("Could not open %s for output\n", output_filename);
      exit(1);
    }
    fprintf(output_fp, "n_fish: %d\n", n_fish);
  }

  init_fish();

  initialize_timer (&total_timer);
  start_timer(&total_timer);

  for (output_time = 0.0, curr_time = 0.0, steps = 0;
       curr_time <= end_time && steps < max_steps;
       curr_time += dt, ++steps) {

    /* We only output once every output_interval time unit, at most.
     * Without that restriction, we can easily create a huge output
     * file.  Printing a record for ten fish takes about 300 bytes,
     * so for every 1000 steps, we could dump 300K of info.  Now
     * scale the number of fish by 1000...
     */
    if (outputp && curr_time >= output_time) {
      output_fish(output_fp, curr_time, dt, fish, n_fish);
      output_time = curr_time + output_interval;
    }

    interact_fish(fish, n_fish, dt);

    max_norm = compute_norm(fish, n_fish);
    dt = max_norm_change / max_norm;
    dt = f_max(dt, min_dt);
    dt = f_min(dt, max_dt);

    move_fish(fish, n_fish, dt);
  }

  stop_timer(&total_timer);

  if (outputp) {
    output_fish(output_fp, curr_time, dt, fish, n_fish);
  }

  printf("Ended at %g (%g), %d (%d) steps\n",
      curr_time, end_time, steps, max_steps);
  printf("Total time taken: %g\n", timer_duration(total_timer));

  return 0;
}


/* Compute the global maximum of all velocities / accelerations.  */
double
compute_norm (const fish_t* fish, const int n_fish)
{
  int i;
  double max_norm = 0.0;

  for (i = 0; i < n_fish; ++i) {
    max_norm = f_max (max_norm, fabs(fish[i].vx));
    max_norm = f_max (max_norm, fabs(fish[i].vy));
    max_norm = f_max (max_norm, fabs(fish[i].ax));
    max_norm = f_max (max_norm, fabs(fish[i].ay));
  }

  return max_norm;
}

/* Compute the accelerations (force/mass) for each fish */
void
interact_fish(fish_t* fish_, const int n_fish, const double dt)
{
  fish_t *__restrict__ fish = fish_;
  
  int i, j;
  double r2, r16, r32, r4, r, a, angle, vtempx, vtempy;
  int in_sight, in_crowd;
  
  
  for (i = 0; i < n_fish; ++i) {
    
    v0x = v0y = v1x = v1y = v2x = v2y = v3x = v3y = v4x = v4y = v5x = v5y = vx = vy = ax = ay = 0.0;
    in_sight = in_crowd = 0;
    local_cogx = 0.0;
    local_cogy = 0.0;    

    // calculate random unit vector
    v4x = (drand48()-0.5);
    v4y = (drand48()-0.5);
    r2 = f_max(v4x * v4x + v4y * v4y, min_r2);
    r = sqrt(r2);
    v4x /= r;
    v4y /= r;    

    for (j = 0; j < n_fish; ++j) {
      double dx, dy;

      dx = fmod((fish[j].x - fish[i].x), (2.0 - R_SIGHT));
      dy = fmod((fish[j].y - fish[i].y), (2.0 - R_SIGHT));

      if (dx != 0 || dy != 0) {	
	r2 = f_max(dx * dx + dy * dy, min_r2);
	
	// figure out if the boid is in this boid's range of vision
	angle = fabs( atan2(dy, dx) - atan2(fish[i].vy, fish[i].vx) );
	
	if( (angle <= (SIGHT_ANGLE/2.0)) || (angle >= (2*M_PI - SIGHT_ANGLE/2.0)) ) {
	  // calculate crowding vector
	  if ( r2 < R_CROWD2 ) {
	    v0x -= dx;
	    v0y -= dy;
	    in_crowd++;
	  }
	  
	  // calculate flocking vector
	  if( r2 < R_SIGHT2 ) {
	    v2x += fish[j].vx;
	    v2y += fish[j].vy;
	    
	    local_cogx += fish[j].x;
	    local_cogy += fish[j].y;
	    
	    in_sight++;
	  }
	  
	}	
      }
    }
    
    
    if( in_sight != 0 ) {
      // calculate vector pointing in the direction of the average velocity of nearby flockmates
      v2x /= in_sight;
      v2y /= in_sight;
          
      v2x = v2x - fish[i].vx;
      v2y = v2y - fish[i].vy;

      local_cogx /= in_sight;
      local_cogy /= in_sight;
      
      // calculate vector towards cog of local flockmates
      v5x = local_cogx - fish[i].x;
      v5y = local_cogy - fish[i].y;
    }

    if( in_crowd != 0 ) {
      v0x /= in_crowd;
      v0y /= in_crowd;
      
      r2 = v0x * v0x + v0y * v0y;

      // let the repulsion decrease with r2?
      v0x /= r2*r2;
      v0y /= r2*r2;
    }

    // now we have v0 to v5...
    vx = C0*v0x + C1*v1x + C2*v2x + C3*v3x + C4*v4x + C5*v5x;
    vy = C0*v0y + C1*v1y + C2*v2y + C3*v3y + C4*v4y + C5*v5y;
    
    ax = M*vx;
    ay = M*vy;

    fish[i].ax = ax;
    fish[i].ay = ay;
  }
}


/* Allocate and initialize the fish positions / velocities / accelerations. */
void
init_fish()
{
  int i;
  fish = (fish_t *) malloc(n_fish * sizeof(fish_t));
  for (i = 0; i < n_fish; ++i) {
    if (uniformp) {
      fish[i].x = unscale_coord(drand48());
      fish[i].y = unscale_coord(drand48());
    } else {
      const double angle = i * (2.0 * M_PI / n_fish);
      fish[i].x = unscale_coord(0.5 * cos(angle) + 0.5);
      fish[i].y = unscale_coord(0.5 * sin(angle) + 0.5);
    }
    fish[i].ax = fish[i].ay = 0.0;
    fish[i].vx = START_CONST*(drand48() - 0.5);
    fish[i].vy = START_CONST*(drand48() - 0.5);
  }
}


/* Apply reflective boundary conditions (fish bounce off walls). */
void
bounce_fish (fish_t* fish)
{
  while (fish->x < LEFT_WALL || fish->x > RIGHT_WALL) {
    if (fish->x < LEFT_WALL) {
      fish->x = fish->x + (RIGHT_WALL - LEFT_WALL);
      //fish->x = 2.0 * LEFT_WALL - fish->x;
      //fish->vx = -fish->vx;
    }
    if (fish->x > RIGHT_WALL) {
      fish->x = fish->x - (RIGHT_WALL - LEFT_WALL);
      //fish->x = 2.0 * RIGHT_WALL - fish->x;
      //fish->vx = -fish->vx;
    }
  }
  while (fish->y < LEFT_WALL || fish->y > RIGHT_WALL) {
    if (fish->y < LEFT_WALL) {
      fish->y = fish->y + (RIGHT_WALL - LEFT_WALL);
      //fish->y = 2.0 * LEFT_WALL - fish->y;
      //fish->vy = -fish->vy;
    }
    if (fish->y > RIGHT_WALL) {
      fish->y = fish->y - (RIGHT_WALL - LEFT_WALL);
      //fish->y = 2.0 * RIGHT_WALL - fish->y;
      //fish->vy = -fish->vy;
    }
  }
}


/* Actually move the fish. */
void
move_fish(fish_t* fish, const int n_fish, const double dt)
{
  int i;
  for (i = 0; i < n_fish; ++i) {
    fish[i].x += dt * fish[i].vx * SCALE_FACTOR;
    fish[i].y += dt * fish[i].vy * SCALE_FACTOR;
    fish[i].vx += dt * fish[i].ax;
    fish[i].vy += dt * fish[i].ay;
    bounce_fish(&fish[i]);
  }
}


/*
  Dump out all the fishies (and their center of gravity)
  in a format that the viewer understands.
*/
void
output_fish(FILE* output_fp, const double t, const double dt,
	    const fish_t* fish, const int n_fish)
{
  int i;
  double cg_x = 0.0;
  double cg_y = 0.0;

  fprintf(output_fp, "%.5g (%.5g):\n", t, dt);
  for (i = 0; i < n_fish; ++i) {
    cg_x += fish[i].x;
    cg_y += fish[i].y;
    fprintf(output_fp, "  %d: (%g, %g)\n", i,
	    scale_coord(fish[i].x), scale_coord(fish[i].y));
  }
  cg_x /= n_fish;
  cg_y /= n_fish;
  fprintf(output_fp, "  cg: (%g, %g)\n", scale_coord(cg_x),
	  scale_coord(cg_y));
}


